import rclpy
import cv2
from rclpy.node import Node
from sensor_msgs.msg import Image, CompressedImage
import numpy as np

class ImageProcessorNode(Node):
    def __init__(self):
        super().__init__('image_processor')
        self.get_logger().info("Starting image processor node")
        self.image_sub = self.create_subscription(Image, 'image', self.image_callback, 10)
        self.pub_cropped = self.create_publisher(CompressedImage, '/image/cropped', 10)
    
    def image_callback(self, msg):
        self.get_logger().info("Processing image callback")
        try:
            # Convert ROS Image message to OpenCV image
            np_arr = np.frombuffer(msg.data, dtype=np.uint8)
            image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)

            # Resize the image
            # new_size = (256, 192)
            new_size = (150, 150)
            image_new = cv2.resize(image, dsize=new_size)

            # Convert the resized OpenCV image to a ROS CompressedImage message
            ret, buffer = cv2.imencode('.jpg', image_new)
            crop_msg = CompressedImage()
            crop_msg.header = msg.header
            crop_msg.format = "jpeg"
            crop_msg.data = np.array(buffer).tobytes()

            # Publish the compressed image
            self.pub_cropped.publish(crop_msg)
        
        except Exception as e:
            self.get_logger().error(f"Error processing image: {e}")

def main(args=None):
    rclpy.init(args=args)
    node = ImageProcessorNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()
